/*
* dma.c
*
*  Created on: 2021��5��31��
*      Author: Administrator
*/

#include "../utility.h"
#include "dma.h"
#include "../ext_uart/ext_uart.h"

#define config_64bit_bigend

static XAxiDma AxiDma;		/* Instance of the XAxiDma */

static DmaStatus xDmaStatus;

static QueueHandle_t xEventQueue_tmp;

u32 s2mm_length(XAxiDma *AxiDmaInst){
	u32 data = XAxiDma_ReadReg(AxiDmaInst->RegBase, 0x00000058);
	return data;
}

u32 mm2s_length(XAxiDma *AxiDmaInst){
	u32 data = XAxiDma_ReadReg(AxiDmaInst->RegBase, 0x00000028);
	return data;
}

int start_dma_rx(){
	int Status;
	Status = XAxiDma_SimpleTransfer(&AxiDma,(UINTPTR) (xDmaStatus.RxBufferPtr),
				MAX_RAPIDIO_LEN, XAXIDMA_DEVICE_TO_DMA);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	return Status;
}

static u8 rapidIO_tx_idx = 0; // rapidIO test code
static void TxIntrHandlerISR(void *Callback)
{
	QueueHandle_t xEventQueue = (QueueHandle_t)Callback;
	u32 IrqStatus;
	int TimeOut;
	XAxiDma *AxiDmaInst = (XAxiDma *)(&AxiDma);

	/* Read pending interrupts */
	IrqStatus = XAxiDma_IntrGetIrq(AxiDmaInst, XAXIDMA_DMA_TO_DEVICE);

	/* Acknowledge pending interrupts */
	XAxiDma_IntrAckIrq(AxiDmaInst, IrqStatus, XAXIDMA_DMA_TO_DEVICE);

	/*
	 * If no interrupt is asserted, we do not do anything
	 */
	if (!(IrqStatus & XAXIDMA_IRQ_ALL_MASK)) {
		return;
	}

	/*
	 * If error interrupt is asserted, raise error flag, reset the
	 * hardware to recover from the error, and return with no further
	 * processing.
	 */
	if ((IrqStatus & XAXIDMA_IRQ_ERROR_MASK)) {
		xDmaStatus.Error = 1;
		/*
		 * Reset should never fail for transmit channel
		 */
		XAxiDma_Reset(AxiDmaInst);
		TimeOut = RESET_TIMEOUT_COUNTER;
		while (TimeOut) {
			if (XAxiDma_ResetIsDone(AxiDmaInst)) {
				break;
			}
			TimeOut -= 1;
		}

		if(xQueueIsQueueFullFromISR(xEventQueue) == pdTRUE){
			return;
		}
		portBASE_TYPE xHigherPriorityTaskWoken;
  		xHigherPriorityTaskWoken = pdFALSE;
		Event event;
		makeEvent(&event, 0, EVENT_DMA_ERROR, NULL);
		xQueueSendFromISR( xEventQueue, &event, xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
		return;
	}

	/*
	 * If Completion interrupt is asserted, then set the TxDone flag
	 */
	if ((IrqStatus & XAXIDMA_IRQ_IOC_MASK)) {
		xDmaStatus.TxDone = 1;

		u32 data = mm2s_length(AxiDmaInst);

		if(xQueueIsQueueFullFromISR(xEventQueue) == pdTRUE){
			return;
		}
		portBASE_TYPE xHigherPriorityTaskWoken;
  		xHigherPriorityTaskWoken = pdFALSE;
		Event event;
		
		// u8 *pData = (u8*)pvPortMalloc(sizeof(u8));
		// *pData = rapidIO_tx_idx; // 从队列带出去，报错

		data = rapidIO_tx_idx; // rapidIO test code
		makeEvent(&event, data, EVENT_MM2S, NULL);
		xQueueSendFromISR( xEventQueue, &event, xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
}

static void RxIntrHandlerISR(void *Callback)
{
	QueueHandle_t xEventQueue = (QueueHandle_t)Callback;
	u32 IrqStatus;
	int TimeOut;
	XAxiDma *AxiDmaInst = (XAxiDma *)(&AxiDma);

	/* Read pending interrupts */
	IrqStatus = XAxiDma_IntrGetIrq(AxiDmaInst, XAXIDMA_DEVICE_TO_DMA);

	/* Acknowledge pending interrupts */
	XAxiDma_IntrAckIrq(AxiDmaInst, IrqStatus, XAXIDMA_DEVICE_TO_DMA);

	/*
	 * If no interrupt is asserted, we do not do anything
	 */
	if (!(IrqStatus & XAXIDMA_IRQ_ALL_MASK)) {
		start_dma_rx();
		return;
	}

	/*
	 * If error interrupt is asserted, raise error flag, reset the
	 * hardware to recover from the error, and return with no further
	 * processing.
	 */
	if ((IrqStatus & XAXIDMA_IRQ_ERROR_MASK)) {
		xDmaStatus.Error = 1;
		/* Reset could fail and hang
		 * NEED a way to handle this or do not call it??
		 */
		XAxiDma_Reset(AxiDmaInst);
		TimeOut = RESET_TIMEOUT_COUNTER;
		while (TimeOut) {
			if(XAxiDma_ResetIsDone(AxiDmaInst)) {
				break;
			}
			TimeOut -= 1;
		}

		if(xQueueIsQueueFullFromISR(xEventQueue) == pdTRUE){
			return;
		}
		portBASE_TYPE xHigherPriorityTaskWoken;
  		xHigherPriorityTaskWoken = pdFALSE;
		Event event;
		makeEvent(&event, 1, EVENT_DMA_ERROR, NULL);
		xQueueSendFromISR( xEventQueue, &event, xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);

		return;
	}

	/*
	 * If completion interrupt is asserted, then set RxDone flag
	 */
	if ((IrqStatus & XAXIDMA_IRQ_IOC_MASK)) {
		u32 data = s2mm_length(AxiDmaInst);

		xDmaStatus.RxDone = 1;
		if(xQueueIsQueueFullFromISR(xEventQueue) == pdTRUE){
			return;
		}
		portBASE_TYPE xHigherPriorityTaskWoken;
  		xHigherPriorityTaskWoken = pdFALSE;
		Event event;
		makeEvent(&event, data, EVENT_S2MM, NULL);
		xQueueSendFromISR( xEventQueue, &event, xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
}

int init_axidma(QueueHandle_t xEventQueue){
	int Status = XST_SUCCESS;
	XAxiDma_Config *Config;
	Config = XAxiDma_LookupConfig(DMA_DEV_ID);
	if (!Config) {
		xil_printf("No config found for %d\r\n", DMA_DEV_ID);

		return XST_FAILURE;
	}

	/* Initialize DMA engine */
	Status = XAxiDma_CfgInitialize(&AxiDma, Config);

	if (Status != XST_SUCCESS) {
		xil_printf("Initialization failed %d\r\n", Status);
		return XST_FAILURE;
	}

	if(XAxiDma_HasSg(&AxiDma)){
		xil_printf("Device configured as SG mode \r\n");
		return XST_FAILURE;
	}

    Status = xPortInstallInterruptHandler( XPAR_INTC_0_AXIDMA_0_MM2S_INTROUT_VEC_ID, TxIntrHandlerISR, xEventQueue );
	if( Status != pdPASS )
	{
		return XST_FAILURE;
	}

	Status = xPortInstallInterruptHandler( XPAR_INTC_0_AXIDMA_0_S2MM_INTROUT_VEC_ID, RxIntrHandlerISR, xEventQueue );
	if( Status != pdPASS )
	{
		return XST_FAILURE;
	}

	vPortEnableInterrupt( XPAR_INTC_0_AXIDMA_0_S2MM_INTROUT_VEC_ID );
	vPortEnableInterrupt( XPAR_INTC_0_AXIDMA_0_MM2S_INTROUT_VEC_ID );

	/* Disable all interrupts before setup */

	XAxiDma_IntrDisable(&AxiDma, XAXIDMA_IRQ_ALL_MASK,
						XAXIDMA_DMA_TO_DEVICE);
	XAxiDma_IntrDisable(&AxiDma, XAXIDMA_IRQ_ALL_MASK,
				XAXIDMA_DEVICE_TO_DMA);

	/* Enable all interrupts */
	XAxiDma_IntrEnable(&AxiDma, XAXIDMA_IRQ_ALL_MASK,
							XAXIDMA_DMA_TO_DEVICE);
	XAxiDma_IntrEnable(&AxiDma, XAXIDMA_IRQ_ALL_MASK,
							XAXIDMA_DEVICE_TO_DMA);

	xDmaStatus.RxBufferPtr = (u8 *)pvPortMalloc(sizeof(u8)*MAX_RAPIDIO_LEN);
	xDmaStatus.TxBufferPtr = (u8 *)pvPortMalloc(sizeof(u8)*MAX_RAPIDIO_LEN);
	memset(xDmaStatus.RxBufferPtr, 0x00, MAX_RAPIDIO_LEN);
	memset(xDmaStatus.TxBufferPtr, 0x00, MAX_RAPIDIO_LEN);

	xDmaStatus.rxMsgBufferPtr = (u8 *)pvPortMalloc(sizeof(u8)*MAX_PKT_LEN);
	xDmaStatus.txMsgBufferPtr = (u8 *)pvPortMalloc(sizeof(u8)*MAX_PKT_LEN);
	memset(xDmaStatus.rxMsgBufferPtr, 0x00, MAX_PKT_LEN);
	memset(xDmaStatus.txMsgBufferPtr, 0x00, MAX_PKT_LEN);

	xDmaStatus.Error = 0;
	xDmaStatus.RxDone = 0;
	xDmaStatus.TxDone = 0;
	xDmaStatus.state = STATE_IDLE;
	xDmaStatus.rx_offset = 0;
	xDmaStatus.tx_offset = 0;
	xDmaStatus.doorbell_size = 0;

	xDmaStatus.frameHeader.id = 0;
	xDmaStatus.frameHeader.dir = 0;
	xDmaStatus.frameHeader.data_type = 0;
	xDmaStatus.frameHeader.msg_size = 0;
	xDmaStatus.frameHeader.tx_seq = 0;
	xDmaStatus.frameHeader.transfer_data_type = 0;

	start_dma_rx();

	xEventQueue_tmp = xEventQueue;

	return XST_SUCCESS;
}

/* -------------------- 发送RapidIO -------------------- */

// issue: rapidIO是按64bit的大端模式，DMA会自己调换
void add_RapidIO_header(u8 *HeadBufferPtr, u8 size, u8 ftype, u8 ttype, u8 header_size){
	memset(HeadBufferPtr,0x0,header_size);
	u8 big_end = header_size - 1;
	HeadBufferPtr[big_end-0] = 0x55;

	HeadBufferPtr[big_end-1] = (ftype << 4) + ttype;

	u8 upper_size = ((size-1) & 0xf0) >> 4;
	u8 lower_size = ((size-1) & 0x0f) << 4;
	HeadBufferPtr[big_end-2] = upper_size;
	HeadBufferPtr[big_end-3] = lower_size;
}

int dma_tx(u8 *TxBufferPtr, int tx_length){
	u16 rapidIO_size = 0; // 8/16/32/64/128/256
	u8 header_size = 8;
	u8 ftype = 5;
	u8 ttype = 4;
	memcpy(xDmaStatus.TxBufferPtr + header_size, TxBufferPtr, tx_length);
	if(tx_length < 8+1){
		rapidIO_size = 8;
	}else if(tx_length < 16+1){
		rapidIO_size = 16;		
	}else if(tx_length < 32+1){
		rapidIO_size = 32;		
	}else if(tx_length < 64+1){
		rapidIO_size = 64;		
	}else if(tx_length < 128+1){
		rapidIO_size = 128;		
	}else{
		rapidIO_size = 256;		
	}
	memset(xDmaStatus.TxBufferPtr + header_size + tx_length, 0xf0, rapidIO_size-tx_length);
	add_RapidIO_header(xDmaStatus.TxBufferPtr, rapidIO_size, ftype, ttype, header_size);

	int all_length = rapidIO_size + 8;
	int Status;
	Status = XAxiDma_SimpleTransfer(&AxiDma,(UINTPTR) (xDmaStatus.TxBufferPtr),
				all_length, XAXIDMA_DMA_TO_DEVICE);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	return Status;
}

/* -------------------- 接收RapidIO -------------------- */
int rapidIORxTest(u8 *RxPacket, int rx_length);
// rapidio 大端模式 --- 还有rapidio 额外header
int validRapidIOHeader(u8 *RxBufferPtr, int rx_size);
int frameBufferRapidIo(int rapidIo_type, int rx_size); // RapidIO接收缓存器
int dispatchMsg(u8 *msgPtr, int msgSize);

int process_rx(int rx_length){
#ifdef rapidIO_testFlag
	u8 *RxPacket = (u8 *)pvPortMalloc(sizeof(u8)*rx_length);
	memcpy(RxPacket, xDmaStatus.RxBufferPtr, rx_length);
	// if(rx_length != 8){
	// 	writeToRingBuffer(xDmaStatus.RxBufferPtr, rx_length); // debug ring buffer test code
	// }
	start_dma_rx();

	int ret = rapidIORxTest(RxPacket,rx_length);
	vPortFree(RxPacket);
	return ret;
#endif

#ifndef rapidIO_testFlag
	writeToRingBuffer(xDmaStatus.RxBufferPtr, rx_length); // debug ring buffer test code
	start_dma_rx();

	u8 header_size = 8;
	if(rx_length < 8){
		return -1;
	}
	//check RapidIO header
	int rapidIo_type = validRapidIOHeader(xDmaStatus.RxBufferPtr, rx_length);
	if(rapidIo_type < 0){
		return -2;
	}
	//开始缓存接收的RapidIO数据，判断RapidIO帧类型，确定组帧完成，开始dispatch
	int state = frameBufferRapidIo(rapidIo_type,rx_length);
	if(state != FRAME_COMPLETE){
		return 0;
	}
	int ret = dispatchMsg(xDmaStatus.rxMsgBufferPtr, xDmaStatus.doorbell_size);
	return ret;
#endif
}

int validRapidIOHeader(u8 *RxBufferPtr, int rx_size){
	u8 big_end = 7;
	u8 type = RxBufferPtr[big_end-1]; // 0x54 -- nWRITE  a0 -- doorbell
	int ret = NWRITE;
	switch(type){
		case 0x54:
		{
			u8 upper_size = RxBufferPtr[big_end-2] & 0x0f;
			u8 lower_size = RxBufferPtr[big_end-3] & 0xf0;
			int type_size = (upper_size << 4) + (lower_size >> 4) + 1;
			if(type_size + 8 != rx_size){
				ret = -2;
			}
			break;
		}
		case 0xa0:
		{
			memcpy(&xDmaStatus.doorbell_size, RxBufferPtr+2 , 2);
			ret = DOORBELL;			
			break;
		}
		default:
		{
			ret = -1;
			break;
		}	
	}
	return ret;
}

void swap_64bit_bigend(u8 *srcPtr){
#ifdef config_64bit_bigend
	u8 idx = 0;
	for(idx=0;idx<4;idx++){
		u8 tmp = srcPtr[idx];
		srcPtr[idx] = srcPtr[7-idx];
		srcPtr[7-idx] = tmp;
	}
#endif
}

int parseFrameHeader(u8 *msg_header_bigend){
	memcpy(&xDmaStatus.frameHeader.id, msg_header_bigend+6, 2);
	if(xDmaStatus.frameHeader.id != 0x5555){
		return -1;
	}
	xDmaStatus.frameHeader.dir = msg_header_bigend[5];
	if(xDmaStatus.frameHeader.dir != 0x01){
		return -1;
	}
	xDmaStatus.frameHeader.data_type = msg_header_bigend[4];
	if(xDmaStatus.frameHeader.data_type != 0xA1){
		return -1;
	}
	memcpy(&xDmaStatus.frameHeader.msg_size, msg_header_bigend+2, 2);
	xDmaStatus.frameHeader.tx_seq = msg_header_bigend[1];
	xDmaStatus.frameHeader.transfer_data_type = msg_header_bigend[0];
	return 0;
}

int idle_process(int rapidIo_type, u8 *currentMsgBufferPtr, u16 valid_size){
	if(rapidIo_type == NWRITE){
		if(parseFrameHeader(currentMsgBufferPtr) != 0){
			xDmaStatus.state = STATE_IDLE;
			xDmaStatus.rx_offset = 0;
			return FRAME_HEADER_ERROR;
		}
		xDmaStatus.state = STATE_HEADER_OK;
		xDmaStatus.rx_offset += valid_size;
		return FRAME_OK;
	}else{
		return FRAME_HEADER_ERROR;
	}
}

int header_ok_process(int rapidIo_type, u16 valid_size){
	if(rapidIo_type == NWRITE){
		xDmaStatus.rx_offset += valid_size;
		if(xDmaStatus.frameHeader.msg_size == xDmaStatus.rx_offset){
			xDmaStatus.state = STATE_DATA_OK_WAIT_DOORBELL;
		}
		return FRAME_OK;
	}else{
		xDmaStatus.state = STATE_IDLE;
		xDmaStatus.rx_offset = 0;
		return FRAME_DATA_ERROR;
	}
}

int wait_for_doorbell_process(int rapidIo_type, u8 *currentMsgBufferPtr, u16 valid_size){
	if(rapidIo_type == DOORBELL){
		if(xDmaStatus.doorbell_size == xDmaStatus.rx_offset){
			xDmaStatus.state = STATE_IDLE;
			xDmaStatus.rx_offset = 0;
			return FRAME_COMPLETE;
		}else{
			return FRAME_DOORBELL_ERROR;
		}
	}else{
		xDmaStatus.state = STATE_IDLE;
		xDmaStatus.rx_offset = 0;
		return FRAME_DOORBELL_ERROR;
	}
}

/* -------------------- 接收缓存状态机 -------------------- */
int frameBufferRapidIo(int rapidIo_type, int rx_size){
	u8 *currentMsgBufferPtr = xDmaStatus.rxMsgBufferPtr+xDmaStatus.rx_offset;
	u8 *validBufferPtr = xDmaStatus.RxBufferPtr+8;
	u16 valid_size = rx_size-8; // doorbell不会copy到buffer
	memcpy(currentMsgBufferPtr, validBufferPtr, valid_size);
	start_dma_rx();
	int ret = FRAME_OK;
	switch (xDmaStatus.state)
	{
		case STATE_IDLE:
		{
			ret = idle_process(rapidIo_type, currentMsgBufferPtr, valid_size);
			break;
		}
		case STATE_HEADER_OK:
		{
			ret = header_ok_process(rapidIo_type, valid_size);
			break;
		}
		case STATE_DATA_OK_WAIT_DOORBELL:
		{
			ret = wait_for_doorbell_process(rapidIo_type, currentMsgBufferPtr, valid_size);
			break;
		}
	}
	return ret;
}

/* -------------------- 消息分发 -------------------- */

/* --- 波控寄存器交互 --- */
#define CONTROL_REG 0x44a50000
#define DATA_REG_0  0x44a50010
#define DATA_REG_1  0x44a50014
#define DATA_REG_2  0x44a50018
#define DATA_REG_3  0x44a5001c

u32 combine_4Byte(u8 *msg_idx){
	u32 reg_data = 0x0;
	u32 tmp = 0x0;
	tmp = msg_idx[0];
	reg_data = reg_data + (tmp << 24);
	tmp = msg_idx[1];
	reg_data = reg_data + (tmp << 16);
	tmp = msg_idx[2];
	reg_data = reg_data + (tmp << 8);
	tmp = msg_idx[3];
	reg_data = reg_data + tmp;
	return reg_data;
}

// 未对 valid_size 做处理
void processBeamData(u8 *msg_body, u16 valid_size){
	u32 reg_ctr_val = gw_ReadReg(CONTROL_REG);
	while((reg_ctr_val & 0x8) != 0x0){
		reg_ctr_val = gw_ReadReg(CONTROL_REG);
	}

	u32 reg_0 = combine_4Byte(msg_body);	 
    gw_WriteReg(DATA_REG_0, reg_0);

	reg_0 = combine_4Byte(msg_body+4);	 
    gw_WriteReg(DATA_REG_1, reg_0);

	reg_0 = combine_4Byte(msg_body+8);	 
    gw_WriteReg(DATA_REG_2, reg_0);

	reg_0 = combine_4Byte(msg_body+12);	 
    gw_WriteReg(DATA_REG_3, reg_0);

	reg_ctr_val = gw_ReadReg(CONTROL_REG);
	reg_ctr_val = reg_ctr_val | 0x8;
	gw_WriteReg(CONTROL_REG, reg_ctr_val);
}

int dispatchMsg(u8 *msgPtr, int msgSize){
	u16 block_64bit_num = msgSize / 8;
	int block_idx;
	for(block_idx=0;block_idx<block_64bit_num;block_idx++){
		swap_64bit_bigend(msgPtr+block_idx*8);
	}
	//除去应用数据头
	u8 *msg_body = msgPtr + 8;
	u16 msg_valid_body_size = xDmaStatus.frameHeader.msg_size - 8;
	switch(xDmaStatus.frameHeader.transfer_data_type){
		case 0x01: //天线信号处理 gw
		{
		 	break;
		}
		case 0x02: //天线单元 ext_uart
		{
			pushExtUartTxQueue( msg_body, msg_valid_body_size); //阻塞操作移交 BlockExtUartTxTask
			break;
		}
		case 0x03: // 波控数据 beam 寄存器交互
		{
			processBeamData(msg_body, msg_valid_body_size);
			break;
		}
	}
	return 0;
}

/* ---------------------------------- test interface ---------------------------------- */

void enable_tx_test(u32 tx_cnt, u32 clear_cnt){
	rapidIO_tx_idx = 0; // rapidIO test code
	Event event;
	u32 *data_array = (u32*)pvPortMalloc(2*sizeof(u32));
	data_array[0] = tx_cnt;
	data_array[1] = clear_cnt;
	makeEvent(&event, 2, EVENT_DMA_TEST_ENABLE, data_array);
	xQueueSend( xEventQueue_tmp, &event, 0UL);
}

void copy_test_tx_data(u8 *testBufferPtr, u32 tx_length){
	memcpy(testBufferPtr, xDmaStatus.TxBufferPtr, tx_length);
}


#ifdef rapidIO_testFlag
/* rapidIO test */
u64 rapid_io_1[33] = {
0x1e542ff000000000,
0x5555555500000410,
0x0002000115f10001,
0x00000000dddddddd,
0x1b85000028000000,
0x3a3b3c3d3e3f4041,
0x4243444546474849,
0x4a4b4c4d4e4f5051,
0x5253545556575859,
0x5a5b5c5d5e5f6061,
0x6263646566676869,
0x6a6b6c6d6e6f7072,
0x7374757677787879,
0x7a7b7c7d7e7f8082,
0x8384858687888889,
0x8a8b8c8d8e8f9091,
0x9293949596979899,
0x9a9b9c9d9e9fa0a1,
0xeeeeeeeeeeeeeeee,
0xffffffffffffffff,
0x0001020304050607,
0x08090a0b0c0d0e0f,
0x1011121314151617,
0x18191a1b1c1d1e1f,
0x2021222324252627,
0x28292a2b2c2d2e2f,
0x3032333435363738,
0x38393a3b3c3d3e3f,
0x4041424344454647,
0x48494a4b4c4d4e4f,
0x5051525354555657,
0x58595a5b5c5d5e5f,
0x6061626364656667
};
u64 rapid_io_2[33] = {
0x1f542ff000000000,
0x68696a6b6c6d6e6f,
0x7072737475767778,
0x78797a7b7c7d7e7f,
0x8082838485868788,
0x88898a8b8c8d8e8f,
0x9091929394959697,
0x98999a9b9c9d9e9f,
0xcccccccccccccccc,
0x0102030405060708,
0x090a0b0c0d0e0f10,
0x1112131415161718,
0x191a1b1c1d1e1f20,
0x2122232425262728,
0x292a2b2c2d2e2f30,
0x3233343536373838,
0x393a3b3c3d3e3f40,
0x4142434445464748,
0x494a4b4c4d4e4f50,
0x5152535455565758,
0x595a5b5c5d5e5f60,
0x6162636465666768,
0x696a6b6c6d6e6f70,
0x7273747576777878,
0x797a7b7c7d7e7f80,
0x8283848586878888,
0x898a8b8c8d8e8f90,
0x9192939495969798,
0x999a9b9c9d9e9fa0,
0xdddddddddddddddd,
0x0203040506070809,
0x0a0b0c0d0e0f1011,
0x1213141516171819
};
u64 rapid_io_3[33] = {
0x20542ff000000000,
0x1a1b1c1d1e1f2021,
0x2223242526272829,
0x2a2b2c2d2e2f3032,
0x3334353637383839,
0x3a3b3c3d3e3f4041,
0x4243444546474849,
0x4a4b4c4d4e4f5051,
0x5253545556575859,
0x5a5b5c5d5e5f6061,
0x6263646566676869,
0x6a6b6c6d6e6f7072,
0x7374757677787879,
0x7a7b7c7d7e7f8082,
0x8384858687888889,
0x8a8b8c8d8e8f9091,
0x9293949596979899,
0x9a9b9c9d9e9fa0a1,
0xeeeeeeeeeeeeeeee,
0xffffffffffffffff,
0x0001020304050607,
0x08090a0b0c0d0e0f,
0x1011121314151617,
0x18191a1b1c1d1e1f,
0x2021222324252627,
0x28292a2b2c2d2e2f,
0x3032333435363738,
0x38393a3b3c3d3e3f,
0x4041424344454647,
0x48494a4b4c4d4e4f,
0x5051525354555657,
0x58595a5b5c5d5e5f,
0x6061626364656667
};
u64 rapid_io_4[33] = {
0x21542ff000000000,
0x68696a6b6c6d6e6f,
0x7072737475767778,
0x78797a7b7c7d7e7f,
0x8082838485868788,
0x88898a8b8c8d8e8f,
0x9091929394959697,
0x98999a9b9c9d9e9f,
0xcccccccccccccccc,
0x0102030405060708,
0x090a0b0c0d0e0f10,
0x1112131415161718,
0x191a1b1c1d1e1f20,
0x2122232425262728,
0x292a2b2c2d2e2f30,
0x3233343536373838,
0x393a3b3c3d3e3f40,
0x4142434445464748,
0x494a4b4c4d4e4f50,
0x5152535455565758,
0x595a5b5c5d5e5f60,
0x6162636465666768,
0x696a6b6c6d6e6f70,
0x7273747576777878,
0x797a7b7c7d7e7f80,
0x8283848586878888,
0x898a8b8c8d8e8f90,
0x9192939495969798,
0x999a9b9c9d9e9fa0,
0xdddddddddddddddd,
0x0203040506070809,
0x0a0b0c0d0e0f1011,
0x1213141516171819
};
u64 rapid_io_5[3] = {0x225420f000000000,0x1a1b1c1d1e1f2021,0xaaaaaaaaaaaaaaaa};
u64 doorbell[1]   = {0x23a0200004100000};

// sendEn
u64 send_en_1[5] = {0xd85421f000000000,0x5555555500000038,0x0000000015f90001,0xffffffffffffffff,0x059d000000000000};
u64 send_en_2[3] = {0xd95420f000000000,0x2801aaaaaaaaaaaa,0xaaaaaaaaaaaaaaaa};
u64 send_en_doorbell[1] = {0xdaa0200000300000};

u64* rapid_packets[6]      = {rapid_io_1,rapid_io_2,rapid_io_3,rapid_io_4,rapid_io_5,doorbell};
u16  rapid_packets_size[6] = {264,       264,       264,       264,       24,        8};
u16  rapidPacketsSize64[6] = {33,        33,        33,        33,         3,        1};

u64* sendEn_packets[3]      = {send_en_1,send_en_2,send_en_doorbell};
u16  sendEn_packets_size[3] = {40,       24,        8};
u16  sendEnPacketsSize64[3] = {5,         3,        1};

int dmaTxRapidIOContinue(u8 rapidIoPacketIdx, u8 *sendEnFlagPtr, u8 *sendEnCntPtr, u32 *tx120packetCntPtr, State *xState){

	u8 sendEnFlag = *sendEnFlagPtr;
	u8 sendEnCnt  = *sendEnCntPtr;

	if(!sendEnFlag){
		rapidIO_tx_idx = rapidIoPacketIdx;

		u64 *packetBufferPtr = rapid_packets[rapidIoPacketIdx];
		u8* txBufferPtr = (u8*)packetBufferPtr;
		u16 bufferSize = 0;

		if(rapidIoPacketIdx == 0){
			bufferSize = 264;
			xState->dma_state.debug_cnt_state.rapid_io_1_cnt ++;
		}else if(rapidIoPacketIdx == 1){
			bufferSize = 264;
			xState->dma_state.debug_cnt_state.rapid_io_2_cnt ++;		
		}else if(rapidIoPacketIdx == 2){
			bufferSize = 264;
			xState->dma_state.debug_cnt_state.rapid_io_3_cnt ++;		
		}else if(rapidIoPacketIdx == 3){
			bufferSize = 264;
			xState->dma_state.debug_cnt_state.rapid_io_4_cnt ++;		
		}else if(rapidIoPacketIdx == 4){
			bufferSize = 24;
			xState->dma_state.debug_cnt_state.rapid_io_5_cnt ++;
		}else if(rapidIoPacketIdx == 5){
			bufferSize = 8;
			xState->dma_state.debug_cnt_state.doorbell_cnt ++;
		}

		(*tx120packetCntPtr)++;

		if( (*tx120packetCntPtr) == (xState->dma_state.clear_cnt * 6) && (*sendEnFlagPtr) == 0 ){
			*tx120packetCntPtr = 0;
			*sendEnFlagPtr = 1;
		}

		int Status;
		Status = XAxiDma_SimpleTransfer(&AxiDma,(UINTPTR) (txBufferPtr),
					bufferSize, XAXIDMA_DMA_TO_DEVICE);
		if (Status != XST_SUCCESS) {
			return XST_FAILURE;
		}
		return XST_SUCCESS;
	}else{
		u64 *packetBufferPtr = sendEn_packets[sendEnCnt];
		u8* txBufferPtr = (u8*)packetBufferPtr;
		u16 bufferSize = 0;
		if(sendEnCnt == 0){
			bufferSize = 40;
			*(sendEnCntPtr) = 1;
			xState->dma_state.debug_cnt_state.send_en_1_cnt ++;
		}else if(sendEnCnt == 1){
			bufferSize = 24;
			*(sendEnCntPtr) = 2;
			xState->dma_state.debug_cnt_state.send_en_2_cnt ++;
		}else if(sendEnCnt == 2){
			bufferSize = 8;
			xState->dma_state.debug_cnt_state.send_en_doorbell_cnt ++;
			xState->dma_state.debug_cnt_state.send_en_doorbell_flag = 1;
			*sendEnFlagPtr = 0;
			*sendEnCntPtr = 0;
			rapidIO_tx_idx = 0;
		}

		int Status;
		Status = XAxiDma_SimpleTransfer(&AxiDma,(UINTPTR) (txBufferPtr),
					bufferSize, XAXIDMA_DMA_TO_DEVICE);
		if (Status != XST_SUCCESS) {
			return XST_FAILURE;
		}
		return XST_SUCCESS;	
	}
}

static u8 rapidIO_rx_idx = 0;
int rapidIORxTest(u8 *RxPacket, int rx_length){
	u64 *expectPacketBufferPtr = rapid_packets[rapidIO_rx_idx];

	u16 expectLength = rapid_packets_size[rapidIO_rx_idx];
	u16 expectLength64 = rapidPacketsSize64[rapidIO_rx_idx];

	rapidIO_rx_idx = rapidIO_rx_idx + 1;
	if(rapidIO_rx_idx == 6){
		rapidIO_rx_idx = 0;
	}

	if(expectLength != rx_length){
		return 1;
	}

	u64* tmpData;
	u64 cmpData;
	if(expectLength == 8){
		tmpData = (u64*)(RxPacket);
		// cmpData = *tmpData - expectPacketBufferPtr[0];
		cmpData = (*tmpData & 0x00ff000000000000) - 0x00a0000000000000;
		if(cmpData != 0 ){
			return 2;
		}
		cmpData = (*tmpData & 0x000000000ff00000) - 0x0000000004100000;
		if(cmpData != 0 ){
			return 3;
		}		
		return 0;
	}

	if(expectLength > 8){
		tmpData = (u64*)(RxPacket+8);
		cmpData = *tmpData - expectPacketBufferPtr[1];
		if(cmpData != 0){
			return 4;
		}
		tmpData = (u64*)(RxPacket+expectLength-8);
		cmpData = *tmpData - expectPacketBufferPtr[expectLength64-1];
		if(cmpData != 0){
			return 5;
		}
	}

	return 0;
}
#endif
